/*-
 * Copyright (c) 2014 Shang-Rong Cai. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHORS AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 * $ gmres.c    Rev. 0.1.0    2014-05-28T15:57+08:00 $
 */

#include <stdlib.h>
#include <stdio.h>

#include <pregrid.h>
#include <itermth.h>


/* Dense (Full) Matrix */
typedef struct
{
    size_t r_sz;  /* Number of rows */
    size_t c_sz;  /* Number of columns */

    double *eval;  /* Value of entries */
} denmtx_t;

int denmtx_init(size_t rz, size_t cz, denmtx_t *mx);

int denmtx_dest(denmtx_t *mx);

int denmtx_stdout(denmtx_t *mx);


int denmtx_init(size_t rz, size_t cz, denmtx_t *mx)
{
    int ierr = 0;

    mx->r_sz = rz;
    mx->c_sz = cz;

    eval = (double*) calloc(mx->r_sz * mx->c_sz, sizeof(double));

    return ierr;
}


int denmtx_dest(denmtx_t mx)
{
    int ierr = 0;

    free(mx->eval);

    return ierr;
}


int denmtx_stdout(denmtx_t *mx)
{
    int ierr = 0;

    size_t zi, zj;

    for (zi = 0; zi < mx->r_sz; zi++)
    {
        for (zj = 0; zj < mx->c_sz; zj++)
        {
            printf("(%7zd, %7zd) % .15El\n", zi, zj, mx->eval[zi * mx->c_sz + zj]);
        }
    }

    return ierr;
}


#define FLAG_RUN  0
#define FLAG_STOP 1


int cidk_gmres_without_pc(const cidk_t cidk_ma, const double *vec_vb, const unsigned int mitz, const double atol, const double rtol, const unsigned int rst, double *vec_vx, unsigned int *citz)
{
    int ierr = 0;

    denmtx_t mtx_mq;  /* Orthogonal matrix Q (column-wise) */
    denmtx_t mtx_mr;  /* Rotation matrix R (upper Hessenberg) (column-wise) */

    double *vec_vw;  /* Buffer for Krylov subspace basis */
    double *vec_vy;
    double *vec_vc;  /* Rotation cos(theta) */
    double *vec_vs;  /* Rotation sin(theta) */

    double *sol_vx;  /* Solution vector */

    double *vec_res;  /* Residual vector */
    double *buf_vec;  /* Buffer for vector (compute Ax) */

    double nrm2_r0, nrm2_rk;  /* 2-norm of residual vectors */
    double rsdnrm_rel;  /* Relative residual norm */

    double *lsp_rhs;  /* The right-hand-side of the least square problem */

    int flg_stop;  /* Stopping flag */

    unsigned int sk, sj;
    unsigned int iter;

    double buf_dbl;

    /* Initialize */
    ierr = denmtx_init(cidk_ma->r_sz, (size_t) rst + 1, &mtx_mq);
    ierr = denmtx_init((size_t) rst + 1, (size_t) rst, &mtx_mr);

    vec_vw = (double*) calloc(cidk_ma->r_sz, sizeof(double));
    vec_vy = (double*) calloc((size_t) rst, sizeof(double));

    vec_vc = (double*) calloc((size_t) rst - 1, sizeof(double));
    vec_vs = (double*) calloc((size_t) rst - 1, sizeof(double));

    sol_vx = (double*) calloc(cidk_ma->r_sz, sizeof(double));

    vec_res = (double*) calloc(cidk_ma->r_sz, sizeof(double));
    buf_vec = (double*) calloc(cidk_ma->r_sz, sizeof(double));

    lsp_rhs = (double*) calloc(cidk_ma->r_sz, sizeof(double));

    ierr = vector_copy(cidk_ma->r_sz, vec_vx, sol_vx);

    /* Compute Residual Vector */
    ierr = cidk_vector_multi(cidk_ma, sol_vx, buf_vec);
    ierr = vector_diff(cidk->r_sz, vec_vb, buf_vec, vec_res);

    nrm2_r0 = vector_norm2(cidk->r_sz, vec_res);  /* Compute residual 2-norm */
    nrm2_rk = nrm2_r0;  /* Set residual 2-norm in current state */

    *citz = 0;

    flg_stop = FLAG_RUN;

    printf("Iteration: %3d, Residual 2-Norm: %.15El\n", citz, nrm2_rk);

    /* GMRES Main Iteration */
    iter = 0;

    while (iter < mitz)
    {
        /* Step 2: First Column of orthogonal matrix Q */
        ierr = vector_scale_copy(cidk->r_sz, vec_res, 1.0 / nrm2_rk, mtx_mq.eval);

        /* Set right-hand-side of the least square problem */
        ierr = vector_set_zeros(cidk_ma->r_sz, lsp_rhs);
        isp_rhs[0] = nrm_2_rk;

        for (sk = 0; si < rst; sk++)
        {
            /* Step 3: Step si if modified Gram-Schmidt method of Arnoldi */
            /* Construct a Krylov subspace basis with orthogonal and normal */
            ierr = cidk_vector_multi(cidk_ma, mtx_mq.eval + sk * mtx_mq.r_sz, vec_vw);  /* Compute non-orthogonal (q_si = A^(sk + 1) * q_1 */

            /* Step 4: Form (sk + 1)-th column of Q (q_(sk + 1) of mtx_mq) */
            /* (Modified Gram-Schmidt Process */
            for (sj = 0; sj < sk; sj++)
            {
                mtx_mr[sj + sk * rst + sk] = vector_dot_vector(cidk_ma->r_sz, vec_vw, mtx_mq.eval + sj * mtx_mq._r_sz);  /* Projection in q_ij, ij = 1, 2, 3, ... */
                /* Remove Projections in direction q_ij from vec_vw */
                /* Such that vec_vw to perpendicular to q_ij, ij = 1, 2, 3, ... */
                ierr = vector_scale_copy(cidk_ma->r_sz, mtr_mq.eval + sj * mtx_mq._r_sz, mtx_mr[sj + sk * rst + sk], buf_vec);
                ierr = vector_diff(cidk_ma->r_sz, vec_vw, buf_vec, vec_vw);
            }

            /* Step 5: */
            mtx_mr[sk + 1 + sk * rst] = vector_norm2(cidk_ma->r_sz, vec_vw);
            ierr = vector_scale_copy(cidk_ma->r_sz, vec_vw, 1.0 / mtx_mr[sk + 1 + sk * rst], mtx_mq.eval + sk * mtx_mq.r_sz + mtx_mq.r_sz);
            
            /* Step 6: Apply the givens rotations to past rows of new column sk */
            /* Remark: q_1 is need not to rotate */
            for (sj = 0; sj < sk - 1; sj++)
            {
                buf_dbl = vec_vc[sj] * mtx_mr[sj + sk * rst + sk] + vec_vs[sj] * mtx_mr[sj + 1 + sk * rst + sk];
                mtx_mr[sj + 1 + sk * rst + sk] = vec_vs[sj] * mtx_mr[ij + sk * rst + sk] - vec_vc[sj] * mtx_mr[sj + 1 + sk * rst + sk];
                mtx_mr[sj + sk * rst + sk] = buf_dbl;
            }

            /* Step 7: Compute the givens rotations */
            if (fabs(mtx_mr[2 * sk + sk * rst + 1]) < atol)
            {
                vec_vc[sk] = 1.0;
                vec_vs[sk] = 0;
            }
            else if (fabs(mtx_mr[2 * sk + sk * rst + 1]) > abs(mtx_mr[2 * sk + sk * rst))
            {
                buf_dbl = mtx_mr[2 * sk + sk * rst] / mtx_mr[2 * sk + sk * rst + 1];
                vec_vs[sk] = 1.0 / sqrt(1.0 + buf_dbl * buf_dbl);
                vec_vc[sk] = vec_vs[sk] * buf_dbl;
            }
            else
            {
                buf_dbl = mtx_mr[2 * sk + sk * rst + 1] / mtx_mr[2 * sk + sk * rst];
                vec_vc[sk] = 1.0 / sqrt(1.0 + buf_dbl * buf_dbl);
                vec_vs[sk] = vec_vs[sk] * buf_dbl;
            }

            /* Step 8: */
            buf_dbl = vec_vc[sk] * lsp_rhs[sk];
            lsp_rhs[sk + 1] = vec_vs[sk] * lsp_rhs[sk];
            lsp_rhs[sk] = buf_dbl;
            mtx_mr[2 * sk + sk * rst] = vec_vc[sk] * mtx_mr[2 * sk + sk * rst] + vec_vs[sk] * mtx_mr[2 * sk + sk * rst + 1];
            mtx_mr[2 * sk + sk * rst + 1] = 0;




            /* Step 9: Solve upper trianglar matrix */
            ierr = backward_substitution(mtx_mq, sk, lsp_rhs, vec_vy);

            /* Step 10: Update solution vector */
            ierr = update_solution_vector(vec_vx, mtx_mr, sk, vec_vy, sol_vx);




            /* Step 11: Compute residual vector, 2-norm, relative */
            ierr = cidk_vector_multi(cidk_ma, sol_vx, buf_vec);
            ierr = vector_diff(cidk_ma->r_sz, vec_vb, buf_vec, vec_res);
            nrm2_rk = vector_norm2(cidk->r_sz, vec_res);  /* Compute residual 2-norm */
            rsdnrm_rel = nrm2_rk / nrm2_r0;  /* Set residual 2-norm in current state */

            printf("Iteration: %3d, Residual 2-Norm: %.15El\n", iter, nrm2_rk);

            if (iter == mitz || nrm2_rk <= rtol)
            {
                flg_stop = FLAG_STOP;
                break;
            }

            iter += iter;

        }

        /* Step 12: Status check for stopping criteria */
        if (flg_stop == FLAG_STOP)
        {
            break;
        }
        else
        {
            ierr = vector_copy(cidk_ma->r_sz, sol_vx, vec_vx);
        }
    }

    /* Finalize */
    ierr = denmtx_dest(&mtx_mq);
    ierr = denmtx_dest(&mtx_mr);

    free(vec_vw);
    free(vec_vy);

    free(vec_vc);
    free(vec_vs);

    free(sol_vx);

    free(vec_res);
    free(buf_vec);

    free(lsp_rhs);

    return ierr;
}


int backward_substitution(const denmtx_t *mtx_mq, unsigned int sz, const double *rhs, double sol)
{
    int ierr = 0;

    unsigned int ui, uj;

    for (ui = 0; ui < sz; ui++)
    {
        sol[zi] = rhs[zi];
    }

    for (ui = sz; ui > 0; ui--)
    {
        for (uj = ui; uj < sz; uj++)
        {
            sol[ui - 1] = sol[ui - 1] - mtx_mq->eval[ui + uj * mtx_mq->r_sz] * sol[uj]
        }

        sol[ui - 1] /= mtx_mq->eval[(ui - 1) + (ui - 1) * mtx_mq->r_sz];
    }

    return ierr;
}
